package com.ai.robot;

import com.pi4j.io.gpio.*;
import org.springframework.stereotype.Service;

/**
 * 机器人运作实现类
 *
 * 树莓派GPIO 来实现
 */
@Service
public class RealRobotControl {

    // 定义GPIO引脚，用于控制电机和舵机
    private static final int MOTOR_LEFT_FORWARD_PIN = 17;   // 左电机前进
    private static final int MOTOR_LEFT_BACKWARD_PIN = 18;  // 左电机后退
    private static final int MOTOR_RIGHT_FORWARD_PIN = 22;  // 右电机前进
    private static final int MOTOR_RIGHT_BACKWARD_PIN = 23; // 右电机后退
    private static final int SERVO_LEFT_PIN = 25;           // 左舵机（控制左转）
    private static final int SERVO_RIGHT_PIN = 24;          // 右舵机（控制右转）
    private static GpioController gpio; // GPIO控制器
    private static GpioPinDigitalOutput motorLeftForward;   // 左电机引脚
    private static GpioPinDigitalOutput motorLeftBackward; // 右电机引脚
    private static GpioPinDigitalOutput motorRightForward; // 右电机引脚
    private static GpioPinDigitalOutput motorRightBackward; // 右电机引脚
    private static GpioPinPwmOutput servoLeft;  // 左舵机引脚
    private static GpioPinPwmOutput servoRight; // 右舵机引脚

    /*
    *
    *
    * 代码解释：
        GPIO 控制：

        motorLeftForward、motorLeftBackward、motorRightForward、motorRightBackward 控制电机的前进、后退。
        servoLeft 和 servoRight 控制机器人的舵机，左右转向。


        电机控制：
        通过设置 motorLeftForward 和 motorRightForward 为 HIGH，使电机前进。类似地，设置为 LOW 来停止电机。
        反向电机控制同理，通过 motorLeftBackward 和 motorRightBackward 来后退。


        舵机控制：
        舵机的旋转可以通过 PWM 信号来控制。在本例中，我们通过 setPwm() 方法设置 PWM 值来控制舵机的角度（0 表示不动，100 表示极限角度）。


        控制方法：
        moveForward()：电机前进，舵机不动。
        moveBackward()：电机后退，舵机不动。
        turnLeft()：控制左舵机左转，右舵机不动。
        turnRight()：控制右舵机右转，左舵机不动。
        stop()：所有电机停止，舵机恢复到初始位置。
    *
    *
    * */

    /*
        3. 测试与运行：
        确保你的树莓派连接了电机和舵机，并正确连接到 GPIO 引脚。
        在树莓派上运行这个 Java 程序，程序会依次模拟机器人的前进、后退、左转、右转和停止动作。

        针对上述进一步优化：
        多线程控制：可以使用多线程来处理并行的电机和舵机控制，以便更流畅地执行命令。
        传感器反馈：加入传感器（如超声波传感器）来感知环境并做出反应，进一步增强机器人智能。
        远程控制：通过蓝牙或 Wi-Fi 与手机或计算机远程控制机器人。
        这种代码框架适用于基本的机器人控制，你可以根据自己的硬件配置进一步调整 GPIO 引脚号和控制方式。*/


    /**
     * Test
     * @param args
     */
    public  void main(String[] args) {
        // 初始化GPIO控制器
        gpio = GpioFactory.getInstance();

        // 设置电机引脚为输出模式
        motorLeftForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_00, PinState.LOW);
        motorLeftBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_01, PinState.LOW);
        motorRightForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_02, PinState.LOW);
        motorRightBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_03, PinState.LOW);

        // 设置舵机引脚为PWM输出模式
        servoLeft = gpio.provisionPwmOutputPin(RaspiPin.GPIO_04);
        servoRight = gpio.provisionPwmOutputPin(RaspiPin.GPIO_05);

        // 设置舵机的初始角度
        servoLeft.setPwm(0);   // 左舵机初始位置
        servoRight.setPwm(0);  // 右舵机初始位置

        // 控制机器人动作

        moveForward();  // 前进
        sleep(2000); // 前进2秒
        stop(); // 停止
        sleep(1000); // 停止1秒


        moveBackward(); // 后退
        sleep(2000); // 后退2秒
        stop(); // 停止
        sleep(1000); // 停止1秒


        turnLeft(); // 左转
        sleep(1000); // 左转1秒


        turnRight(); // 右转
        sleep(1000); // 右转1秒
        stop(); // 停止
    }

    // 控制机器人前进
    public void moveForward() {
        setUP();
        motorLeftForward.high();
        motorLeftBackward.low();
        motorRightForward.high();
        motorRightBackward.low();
        moveForward();
        sleep(2000); // 前进2秒
        stop();
        sleep(1000); // 停止1秒
        System.out.println("机器人前进");
    }

    // 控制机器人后退
    public void moveBackward() {

        setUP();

        motorLeftForward.low();
        motorLeftBackward.high();
        motorRightForward.low();
        motorRightBackward.high();
        sleep(2000); // 后退2秒
        stop();
        sleep(1000); // 停止1秒
        stop();
        System.out.println("机器人后退");
    }

    // 控制机器人左转
    public void turnLeft() {

        setUP();

        // 控制舵机左转
        servoLeft.setPwm(100);  // 左舵机控制向左转
        servoRight.setPwm(0);   // 右舵机保持不动
        sleep(1000); // 左转1秒
        stop();
        System.out.println("机器人左转");
    }

    // 控制机器人右转
    public void turnRight() {

        setUP();

        // 控制舵机右转
        servoLeft.setPwm(0);    // 左舵机保持不动
        servoRight.setPwm(100); // 右舵机控制向右转
        sleep(1000); // 右转1秒
        stop(); // 停止
        System.out.println("机器人右转");
    }

    // 控制机器人停止
    public void stop() {
        setUP();
        motorLeftForward.low();
        motorLeftBackward.low();
        motorRightForward.low();
        motorRightBackward.low();
        servoLeft.setPwm(0);   // 停止舵机
        servoRight.setPwm(0);  // 停止舵机
        System.out.println("机器人停止");
    }



    // 睡眠方法，用于控制动作间的延迟
    private static void sleep(long milliseconds) {
        try {
            Thread.sleep(milliseconds);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    /**
     * 电机设置
     */

    public void setUP() {
        // 初始化GPIO控制器
        gpio = GpioFactory.getInstance();

        // 设置电机引脚为输出模式
        motorLeftForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_00, PinState.LOW);
        motorLeftBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_01, PinState.LOW);
        motorRightForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_02, PinState.LOW);
        motorRightBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_03, PinState.LOW);

        // 设置舵机引脚为PWM输出模式
        servoLeft = gpio.provisionPwmOutputPin(RaspiPin.GPIO_04);
        servoRight = gpio.provisionPwmOutputPin(RaspiPin.GPIO_05);

        // 设置舵机的初始角度
        servoLeft.setPwm(0);   // 左舵机初始位置
        servoRight.setPwm(0);  // 右舵机初始位置
    }
}





